{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Populating the interactive namespace from numpy and matplotlib\n"
     ]
    }
   ],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing\n",
    "init_printing(use_latex='mathjax')\n",
    "from sympy import Matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Robotics Toolbox for Python\n",
      "Based on Matlab Toolbox Version 7  April-2002\n",
      "\n",
      "What's new.\n",
      "  Readme      - New features and enhancements in this version.\n",
      "\n",
      "Homogeneous transformations\n",
      "  eul2tr      - Euler angle to transform \n",
      "  oa2tr       - orientation and approach vector to transform \n",
      "  rotx        - transform for rotation about X-axis \n",
      "  roty        - transform for rotation about Y-axis \n",
      "  rotz        - transform for rotation about Z-axis \n",
      "  rpy2tr      - roll/pitch/yaw angles to transform \n",
      "  tr2eul      - transform to Euler angles \n",
      "  tr2rot      - transform to rotation submatrix\n",
      "  tr2rpy      - transform to roll/pitch/yaw angles\n",
      "  transl      - set or extract the translational component of a transform \n",
      "  trnorm      - normalize a transform \n",
      "  \n",
      "Quaternions\n",
      "  /           - divide quaternion by quaternion or scalar\n",
      "  *           - multiply quaternion by a quaternion or vector\n",
      "  inv         - invert a quaternion \n",
      "  norm        - norm of a quaternion \n",
      "  plot        - display a quaternion as a 3D rotation\n",
      "  qinterp     - interpolate quaternions\n",
      "  unit        - unitize a quaternion \n",
      "\n",
      "Kinematics\n",
      "  diff2tr     - differential motion vector to transform \n",
      "  fkine       - compute forward kinematics \n",
      "  ikine       - compute inverse kinematics \n",
      "  ikine560    - compute inverse kinematics for Puma 560 like arm\n",
      "  jacob0      - compute Jacobian in base coordinate frame\n",
      "  jacobn      - compute Jacobian in end-effector coordinate frame\n",
      "  tr2diff     - transform to differential motion vector \n",
      "  tr2jac      - transform to Jacobian \n",
      "  \n",
      "Dynamics\n",
      "  accel       - compute forward dynamics\n",
      "  cinertia    - compute Cartesian manipulator inertia matrix \n",
      "  coriolis    - compute centripetal/coriolis torque \n",
      "  friction    - joint friction\n",
      "  ftrans      - transform force/moment \n",
      "  gravload    - compute gravity loading \n",
      "  inertia     - compute manipulator inertia matrix \n",
      "  itorque     - compute inertia torque \n",
      "  nofriction  - remove friction from a robot object \n",
      "  rne         - inverse dynamics \n",
      "  \n",
      "Trajectory generation\n",
      "  ctraj       - Cartesian trajectory \n",
      "  jtraj       - joint space trajectory \n",
      "  trinterp    - interpolate transform s\n",
      "  \n",
      "Graphics\n",
      "  drivebot    - drive a graphical  robot \n",
      "  plot        - plot/animate robot \n",
      "  \n",
      "Other\n",
      "  manipblty   - compute manipulability \n",
      "  unit        - unitize a vector\n",
      "\n",
      "Creation of robot models.\n",
      "  link        - construct a robot link object \n",
      "  puma560     - Puma 560 data \n",
      "  puma560akb  - Puma 560 data (modified Denavit-Hartenberg)\n",
      "  robot       - construct a robot object \n",
      "  stanford    - Stanford arm data \n",
      "  twolink     - simple 2-link example \n",
      "\n"
     ]
    }
   ],
   "source": [
    "from robot import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "import almath\n",
    "import motion\n",
    "from naoqi import ALProxy\n",
    "frame = motion.FRAME_TORSO\n",
    "useSensorValues = False\n",
    "motionProxy = ALProxy(\"ALMotion\",\"127.0.0.1\",9559)\n",
    "motionProxy.rest()\n",
    "motionProxy.wakeUp()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 基于位置的逆运动学\n",
    "\n",
    "## setPosition"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.118702, y=0.132807, z=-0.0443378, wx=-1.21656, wy=0.415781, wz=0.0102357)"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Pos = motionProxy.getPosition(\"LArm\", frame, useSensorValues)\n",
    "Pos = almath.Position6D(Pos)\n",
    "Pos"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.148749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.589838, wz=0.0127927)"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Pos2 = motionProxy.getPosition(\"LArm\", frame, useSensorValues)\n",
    "Pos2 = almath.Position6D(Pos2)\n",
    "Pos2.x+=0.03 # 3cm\n",
    "Pos2.wy+=deg2rad(10) # 10 度\n",
    "Pos2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "motionProxy.setPositions(\"LArm\",frame,Pos2.toVector(),0.5,almath.AXIS_MASK_ALL)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.127025, y=0.134224, z=-0.0458546, wx=-1.21638, wy=0.460666, wz=0.0134048)"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Pos3 = motionProxy.getPosition(\"LArm\", frame, useSensorValues)\n",
    "Pos3 = almath.Position6D(Pos3)\n",
    "Pos3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.0218109767884$$"
      ],
      "text/plain": [
       "0.0218109767884"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "almath.Position6D.distance(Pos2,Pos3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.198749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.938904, wz=0.0127927)"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Pos2.x+=0.05 # 5cm\n",
    "Pos2.wy+=deg2rad(20) # 10 度\n",
    "Pos2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "motionProxy.setPositions(\"LArm\",frame,Pos2.toVector(),0.5,almath.AXIS_MASK_ALL)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.155195, y=0.131936, z=-0.0415545, wx=-1.21407, wy=0.647907, wz=0.0166492)"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Pos3 = motionProxy.getPosition(\"LArm\", frame, useSensorValues)\n",
    "Pos3 = almath.Position6D(Pos3)\n",
    "Pos3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.043653935194$$"
      ],
      "text/plain": [
       "0.043653935194"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "almath.Position6D.distance(Pos2,Pos3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "##  positionInterpolations 插值, 可实现联动"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "effectorList  = []\n",
    "pathList      = []\n",
    "axisMaskList = [motion.AXIS_MASK_ALL, motion.AXIS_MASK_ALL]\n",
    "timeList     = [[1.0], [1.0,2.0]] # seconds\n",
    "effectorList.append(\"LArm\")\n",
    "pathList.append([Pos.toVector()])\n",
    "effectorList.append(\"RArm\")\n",
    "PosR = motionProxy.getPosition(\"RArm\", frame, useSensorValues)\n",
    "PosR = almath.Position6D(PosR)\n",
    "PosR.x+=0.05\n",
    "PosR.y+=0.05\n",
    "PosR.wz+=deg2rad(10)\n",
    "pathList.append([PosR.toVector(),motionProxy.getPosition(\"RArm\", frame, useSensorValues)])\n",
    "motionProxy.positionInterpolations(effectorList, \n",
    "                                   frame, \n",
    "                                   pathList,\n",
    "                                   axisMaskList,\n",
    "                                   timeList)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.118733, y=0.132815, z=-0.0443444, wx=-1.21656, wy=0.415945, wz=0.0102418)"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "almath.Position6D(motionProxy.getPosition(\"LArm\", frame, useSensorValues))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.118702, y=0.132807, z=-0.0443378, wx=-1.21656, wy=0.415781, wz=0.0102357)"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Pos"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 基于变换的逆运动学\n",
    "\n",
    "## setTransforms"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.914687, -0.3825, 0.130543, 0.168733\n",
       "           0.00936833, 0.342975, 0.939298, 0.132815\n",
       "           -0.404055, -0.85794, 0.317298, 0.00565561])"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "origTransform = almath.Transform(\n",
    "        motionProxy.getTransform(\"LArm\", frame, useSensorValues)) # 起始变换\n",
    "# 将 chain 沿着 Z轴和X轴移动 5cm 的相对变换\n",
    "moveTransform = almath.Transform.fromPosition(0.05, 0.0, 0.05) # 相对运动\n",
    "targetTransform = moveTransform * origTransform\n",
    "targetTransform"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    " motionProxy.setTransforms(\"LArm\", frame, \n",
    "                           targetTransform.toVector(), 0.5, \n",
    "                           motion.AXIS_MASK_ALL)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.914844, -0.38216, 0.130435, 0.126552\n",
       "           0.00936374, 0.343005, 0.939287, 0.134906\n",
       "           -0.403698, -0.85808, 0.317374, -0.0415685])"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "currentTf = almath.Transform(motionProxy.getTransform(\"LArm\", frame, useSensorValues))\n",
    "currentTf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.0633539184928$$"
      ],
      "text/plain": [
       "0.0633539184928"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "almath.Transform.distance(targetTransform,currentTf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.00855661649257$$"
      ],
      "text/plain": [
       "0.00855661649257"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "almath.Transform.distance(origTransform,currentTf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "motionProxy.setTransforms(\"LArm\", frame, \n",
    "                           origTransform.toVector(), 0.5, \n",
    "                           motion.AXIS_MASK_ALL)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.914862, -0.381957, 0.130908, 0.174624\n",
       "           0.00881091, 0.343023, 0.939286, 0.140385\n",
       "           -0.403671, -0.858163, 0.317184, -0.00664913])"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "currentTf2 = almath.Transform(motionProxy.getTransform(\"LArm\", frame, useSensorValues))\n",
    "currentTf2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.0678380057216$$"
      ],
      "text/plain": [
       "0.0678380057216"
      ]
     },
     "execution_count": 23,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "almath.Transform.distance(origTransform,currentTf2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "motionProxy.setTransforms(\"LArm\", frame, \n",
    "                           currentTf.toVector(), 0.5, \n",
    "                           motion.AXIS_MASK_ALL)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.914911, -0.382085, 0.13019, 0.129082\n",
       "           0.00967348, 0.343187, 0.939217, 0.135614\n",
       "           -0.40354, -0.85804, 0.317682, -0.0405014])"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "currentTf3 = almath.Transform(motionProxy.getTransform(\"LArm\", frame, useSensorValues))\n",
    "currentTf3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.00283521786332$$"
      ],
      "text/plain": [
       "0.00283521786332"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "almath.Transform.distance(currentTf,currentTf3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "经过上面的演示可以得出一个结论: 由于Nao的机械臂只有5个电机,实际上只有5个自由度,\n",
    "在计算逆运动学时,只能得到一个距离目标位姿最近的解,若目标位姿在解域内则误差几乎为零,\n",
    "反之则误差很大"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "Pos = motionProxy.getPosition(\"LArm\", frame, useSensorValues)\n",
    "Pos = almath.Position6D(Pos)\n",
    "Pos"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
